//----------------------------------Include-------------------------------------
#include <stdint.h>
#include <math.h>

#include "hpm_math.h"
#include "glbvar.h"
//------------------------------------------------------------------------------

//-----------------------------------Macro--------------------------------------
#define CALC_COL      12     //CALC_COL=2  时是最小二乘法的特例傅氏算法
//------------------------------------------------------------------------------

//------------------------------------Type--------------------------------------
typedef struct{
  float  real;
  float  imag;
}t_cmplx;
//------------------------------------------------------------------------------

//---------------------------------Static Var-----------------------------------
static double        A[GM_SAMP_LEN*CALC_COL];
static double       At[CALC_COL*GM_SAMP_LEN];   //A的转置
static double      AtA[CALC_COL*CALC_COL];      //A的转置乘A
static double     AtAi[CALC_COL*CALC_COL];      //A的转置乘A再取逆
static double   AtAiAt[CALC_COL*GM_SAMP_LEN];   //A的转置乘A取逆再乘A的转置
static double        B[GM_SAMP_LEN*1];
static double        X[CALC_COL*1];
//------------------------------------------------------------------------------

//--------------------------------Static Func-----------------------------------
//------------------------------------------------------------------------------

//---------------------------------Public Func----------------------------------
//设定参数
void calc_init(void)
{
  double    t[GM_SAMP_LEN];
  for(uint32_t i=0; i<GM_SAMP_LEN; i++){
    t[i] = (1.0/GM_BASE_FRQ)/GM_SAMP_LEN*i;
  }

  for(uint32_t i=0; i<GM_SAMP_LEN; i++){
#if CALC_COL == 12
    A[i*12 + 0] = 1;
    A[i*12 + 1] = t[i];
    A[i*12 + 2] = sin(  (2*HPM_MATH_PI*GM_BASE_FRQ)*t[i]);
    A[i*12 + 3] = cos(  (2*HPM_MATH_PI*GM_BASE_FRQ)*t[i]);
    A[i*12 + 4] = sin(2*(2*HPM_MATH_PI*GM_BASE_FRQ)*t[i]);
    A[i*12 + 5] = cos(2*(2*HPM_MATH_PI*GM_BASE_FRQ)*t[i]);
    A[i*12 + 6] = sin(3*(2*HPM_MATH_PI*GM_BASE_FRQ)*t[i]);
    A[i*12 + 7] = cos(3*(2*HPM_MATH_PI*GM_BASE_FRQ)*t[i]);
    A[i*12 + 8] = sin(4*(2*HPM_MATH_PI*GM_BASE_FRQ)*t[i]);
    A[i*12 + 9] = cos(4*(2*HPM_MATH_PI*GM_BASE_FRQ)*t[i]);
    A[i*12 +10] = sin(5*(2*HPM_MATH_PI*GM_BASE_FRQ)*t[i]);
    A[i*12 +11] = cos(5*(2*HPM_MATH_PI*GM_BASE_FRQ)*t[i]);
#endif
#if CALC_COL == 2
    A[i*CALC_COL + 0] = sin((2*HPM_MATH_PI*GM_BASE_FRQ)*t[i]);;
    A[i*CALC_COL + 1] = cos((2*HPM_MATH_PI*GM_BASE_FRQ)*t[i]);
#endif
  }

  hpm_dsp_mat_trans_f64(A, At, GM_SAMP_LEN, CALC_COL);
  hpm_dsp_mat_mul_f64(At, A, AtA, CALC_COL, GM_SAMP_LEN, CALC_COL);
  hpm_dsp_mat_inv_f64(AtA, AtAi, CALC_COL);
  hpm_dsp_mat_mul_f64(AtAi, At, AtAiAt, CALC_COL, CALC_COL, GM_SAMP_LEN);
}

//计算
void calc_para(void)
{
  for(uint32_t i=0; i<GM_SAMP_LEN; i++){
    B[i] = igaf_adc_u[i];
  }

  hpm_dsp_mat_mul_f64(AtAiAt, B, X, CALC_COL, GM_SAMP_LEN, 1);

#if CALC_COL == 12
  gaf_pt_amp_u[1] = hpm_dsp_sqrt_f32(X[2]*X[2] + X[3]*X[3]);
  gaf_pt_amp_u[2] = hpm_dsp_sqrt_f32(X[4]*X[4] + X[5]*X[5]);
  gaf_pt_amp_u[3] = hpm_dsp_sqrt_f32(X[6]*X[6] + X[7]*X[7]);
  gaf_pt_amp_u[4] = hpm_dsp_sqrt_f32(X[8]*X[8] + X[9]*X[9]);
  gaf_pt_amp_u[5] = hpm_dsp_sqrt_f32(X[10]*X[10] + X[11]*X[11]);
#endif
#if CALC_COL == 2
  gaf_pt_amp_u[1] = hpm_dsp_sqrt_f32(X[0]*X[0] + X[1]*X[1]);
#endif
}
//-----------------------------------EOF----------------------------------------

/*
just like signal:
(if use signal generator, notice to multiplying by 2)

      ^
1.0V  |         . .
      |       .     .
      |     .         .
      |   .             .
      |  .               .
0.5V  |-------------------------------------.---------
      |                    .               .
      |                     .             .
      |                       .         .
      |                         .     .
      |                           . .
0V    |=================================================>

gaf_pt_amp_x[0] is DC Component, the value = 0.5
gaf_pt_amp_x[1] is base freq   (50Hz)  amplitude, the value = 0.5, not peak-peak; the rms = 0.5*sqrt(2)/2
gaf_pt_amp_x[2] is base freq*2 (100Hz) amplitude
gaf_pt_amp_x[3] is base freq*3 (150Hz) amplitude

if input square or triangular wave, the amplitude of odd(3,5,7...) is very obvious
*/















